/*
 $License:
    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
 $
 */
 
/*******************************************************************************
 *
 * $Id: mlcontrol.c 5761 2011-07-12 23:17:00Z kpowell $
 *
 *******************************************************************************/

/**
 *  @defgroup   CONTROL
 *  @brief      Motion Library - Control Engine.
 *              The Control Library processes gyroscopes, accelerometers, and 
 *              compasses to provide control signals that can be used in user 
 *              interfaces.
 *              These signals can be used to manipulate objects such as documents,
 *              images, cursors, menus, etc.
 *
 *  @{
 *      @file   mlcontrol.c
 *      @brief  The Control Library.
 *
 */

/* ------------------ */
/* - Include Files. - */
/* ------------------ */

#include "mltypes.h"
#include "mlinclude.h"
#include "mltypes.h"
#include "ml.h"
#include "mlos.h"
#include "mlsl.h"
#include "mldl.h"
#include "mlcontrol.h"
#include "dmpKey.h"
#include "mlstates.h"
#include "mlFIFO.h"
#include "string.h"

/* - Global Vars. - */
struct control_params cntrl_params = {
    {
     MLCTRL_SENSITIVITY_0_DEFAULT,
     MLCTRL_SENSITIVITY_1_DEFAULT,
     MLCTRL_SENSITIVITY_2_DEFAULT,
     MLCTRL_SENSITIVITY_3_DEFAULT}, // sensitivity
    MLCTRL_FUNCTIONS_DEFAULT,   // functions
    {
     MLCTRL_PARAMETER_ARRAY_0_DEFAULT,
     MLCTRL_PARAMETER_ARRAY_1_DEFAULT,
     MLCTRL_PARAMETER_ARRAY_2_DEFAULT,
     MLCTRL_PARAMETER_ARRAY_3_DEFAULT}, // parameterArray
    {
     MLCTRL_PARAMETER_AXIS_0_DEFAULT,
     MLCTRL_PARAMETER_AXIS_1_DEFAULT,
     MLCTRL_PARAMETER_AXIS_2_DEFAULT,
     MLCTRL_PARAMETER_AXIS_3_DEFAULT},  // parameterAxis
    {
     MLCTRL_GRID_THRESHOLD_0_DEFAULT,
     MLCTRL_GRID_THRESHOLD_1_DEFAULT,
     MLCTRL_GRID_THRESHOLD_2_DEFAULT,
     MLCTRL_GRID_THRESHOLD_3_DEFAULT},  // gridThreshold
    {
     MLCTRL_GRID_MAXIMUM_0_DEFAULT,
     MLCTRL_GRID_MAXIMUM_1_DEFAULT,
     MLCTRL_GRID_MAXIMUM_2_DEFAULT,
     MLCTRL_GRID_MAXIMUM_3_DEFAULT},    // gridMaximum
    MLCTRL_GRID_CALLBACK_DEFAULT    // gridCallback
};

/* - Extern Vars. - */
struct control_obj cntrl_obj;
extern const unsigned char *dmpConfig1;

/* -------------- */
/* - Functions. - */
/* -------------- */

/**
 *  @brief  inv_set_control_sensitivity is used to set the sensitivity for a control
 *          signal.
 *
 *  @pre    inv_dmp_open() Must be called with MLDmpDefaultOpen() or 
 *          inv_open_low_power_pedometer().
 *
 *  @param controlSignal    Indicates which control signal is being modified.
 *                          Must be one of:
 *                          - INV_CONTROL_1,
 *                          - INV_CONTROL_2,
 *                          - INV_CONTROL_3 or
 *                          - INV_CONTROL_4.
 *
 *  @param sensitivity      The sensitivity of the control signal.
 *
 *  @return error code
 */
inv_error_t inv_set_control_sensitivity(unsigned short controlSignal,
                                        long sensitivity)
{
    INVENSENSE_FUNC_START;
    unsigned char regs[2];
    long finalSens = 0;
    inv_error_t result;

    if (inv_get_state() < INV_STATE_DMP_OPENED)
        return INV_ERROR_SM_IMPROPER_STATE;

    finalSens = sensitivity * 100;
    if (finalSens > 16384) {
        finalSens = 16384;
    }
    regs[0] = (unsigned char)(finalSens / 256);
    regs[1] = (unsigned char)(finalSens % 256);
    switch (controlSignal) {
    case INV_CONTROL_1:
        result = inv_set_mpu_memory(KEY_D_0_224, 2, regs);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        cntrl_params.sensitivity[0] = (unsigned short)sensitivity;
        break;
    case INV_CONTROL_2:
        result = inv_set_mpu_memory(KEY_D_0_228, 2, regs);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        cntrl_params.sensitivity[1] = (unsigned short)sensitivity;
        break;
    case INV_CONTROL_3:
        result = inv_set_mpu_memory(KEY_D_0_232, 2, regs);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        cntrl_params.sensitivity[2] = (unsigned short)sensitivity;
        break;
    case INV_CONTROL_4:
        result = inv_set_mpu_memory(KEY_D_0_236, 2, regs);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        cntrl_params.sensitivity[3] = (unsigned short)sensitivity;
        break;
    default:
        break;
    }
    if (finalSens != sensitivity * 100) {
        return INV_ERROR_INVALID_PARAMETER;
    } else {
        return INV_SUCCESS;
    }
}

/**
 *  @brief  inv_set_control_func allows the user to choose how the sensor data will
 *          be processed in order to provide a control parameter.
 *          inv_set_control_func allows the user to choose which control functions
 *          will be incorporated in the sensor data processing.
 *          The control functions are:
 *          - INV_GRID
 *          Indicates that the user will be controlling a system that
 *          has discrete steps, such as icons, menu entries, pixels, etc.
 *          - INV_SMOOTH
 *          Indicates that noise from unintentional motion should be filtered out.
 *          - INV_DEAD_ZONE
 *          Indicates that a dead zone should be used, below which sensor
 *          data is set to zero.
 *          - INV_HYSTERESIS
 *          Indicates that, when INV_GRID is selected, hysteresis should
 *          be used to prevent the control signal from switching rapidly across
 *          elements of the grid.
 *
 *  @pre    inv_dmp_open() Must be called with MLDmpDefaultOpen() or 
 *          inv_open_low_power_pedometer().
 *
 *  @param  function    Indicates what functions will be used.
 *                      Can be a bitwise OR of several values.
 *
 *  @return Zero if the command is successful; an ML error code otherwise.
 */
inv_error_t inv_set_control_func(unsigned short function)
{
    INVENSENSE_FUNC_START;
    unsigned char regs[8] = { DINA06, DINA26,
        DINA46, DINA66,
        DINA0E, DINA2E,
        DINA4E, DINA6E
    };
    unsigned char i;
    inv_error_t result;

    if (inv_get_state() < INV_STATE_DMP_OPENED)
        return INV_ERROR_SM_IMPROPER_STATE;

    if ((function & INV_SMOOTH) == 0) {
        for (i = 0; i < 8; i++) {
            regs[i] = DINA80 + 3;
        }
    }
    result = inv_set_mpu_memory(KEY_CFG_4, 8, regs);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }
    cntrl_params.functions = function;
    result = inv_set_dead_zone();

    return result;
}

/**
 *  @brief  inv_get_control_signal is used to get the current control signal with
 *          high precision.
 *          inv_get_control_signal is used to acquire the current data of a control signal.
 *          If INV_GRID is being used, inv_get_grid_number will probably be preferrable.
 *
 *  @param  controlSignal   Indicates which control signal is being queried.
 *          Must be one of:
 *          - INV_CONTROL_1,
 *          - INV_CONTROL_2,
 *          - INV_CONTROL_3 or
 *          - INV_CONTROL_4.
 *
 *  @param  reset   Indicates whether the control signal should be reset to zero.
 *                  Options are INV_RESET or INV_NO_RESET
 *  @param  data    A pointer to the current control signal data.
 *
 *  @return Zero if the command is successful; an ML error code otherwise.
 */
inv_error_t inv_get_control_signal(unsigned short controlSignal,
                                   unsigned short reset, long *data)
{
    INVENSENSE_FUNC_START;

    if (inv_get_state() != INV_STATE_DMP_STARTED)
        return INV_ERROR_SM_IMPROPER_STATE;

    switch (controlSignal) {
    case INV_CONTROL_1:
        *data = cntrl_obj.controlInt[0];
        if (reset == INV_RESET) {
            cntrl_obj.controlInt[0] = 0;
        }
        break;
    case INV_CONTROL_2:
        *data = cntrl_obj.controlInt[1];
        if (reset == INV_RESET) {
            cntrl_obj.controlInt[1] = 0;
        }
        break;
    case INV_CONTROL_3:
        *data = cntrl_obj.controlInt[2];
        if (reset == INV_RESET) {
            cntrl_obj.controlInt[2] = 0;
        }
        break;
    case INV_CONTROL_4:
        *data = cntrl_obj.controlInt[3];
        if (reset == INV_RESET) {
            cntrl_obj.controlInt[3] = 0;
        }
        break;
    default:
        break;
    }
    return INV_SUCCESS;
}

/**
 *  @brief  inv_get_grid_num is used to get the current grid location for a certain
 *          control signal.
 *          inv_get_grid_num is used to acquire the current grid location.
 *
 *  @pre    inv_dmp_open() Must be called with MLDmpDefaultOpen() or 
 *          inv_open_low_power_pedometer().
 *
 *  @param  controlSignal   Indicates which control signal is being queried.
 *          Must be one of:
 *          - INV_CONTROL_1,
 *          - INV_CONTROL_2,
 *          - INV_CONTROL_3 or
 *          - INV_CONTROL_4.
 *
 *  @param  reset   Indicates whether the control signal should be reset to zero.
 *                  Options are INV_RESET or INV_NO_RESET
 *  @param  data    A pointer to the current grid number.
 *
 *  @return Zero if the command is successful; an ML error code otherwise.
 */

inv_error_t inv_get_grid_num(unsigned short controlSignal, unsigned short reset,
                             long *data)
{
    INVENSENSE_FUNC_START;

    if (inv_get_state() != INV_STATE_DMP_STARTED)
        return INV_ERROR_SM_IMPROPER_STATE;

    switch (controlSignal) {
    case INV_CONTROL_1:
        *data = cntrl_obj.gridNum[0];
        if (reset == INV_RESET) {
            cntrl_obj.gridNum[0] = 0;
        }
        break;
    case INV_CONTROL_2:
        *data = cntrl_obj.gridNum[1];
        if (reset == INV_RESET) {
            cntrl_obj.gridNum[1] = 0;
        }
        break;
    case INV_CONTROL_3:
        *data = cntrl_obj.gridNum[2];
        if (reset == INV_RESET) {
            cntrl_obj.gridNum[2] = 0;
        }
        break;
    case INV_CONTROL_4:
        *data = cntrl_obj.gridNum[3];
        if (reset == INV_RESET) {
            cntrl_obj.gridNum[3] = 0;
        }
        break;
    default:
        break;
    }

    return INV_SUCCESS;
}

/**
 *  @brief  inv_set_grid_thresh is used to set the grid size for a control signal.
 *          inv_set_grid_thresh is used to adjust the size of the grid being controlled.
 *  @param  controlSignal   Indicates which control signal is being modified.
 *                          Must be one of:
 *                          - INV_CONTROL_1,
 *                          - INV_CONTROL_2,
 *                          - INV_CONTROL_3 and
 *                          - INV_CONTROL_4.
 *  @param  threshold       The threshold of the control signal at which the grid
 *                          number will be incremented or decremented.
 *  @return Zero if the command is successful; an ML error code otherwise.
 */

inv_error_t inv_set_grid_thresh(unsigned short controlSignal, long threshold)
{
    INVENSENSE_FUNC_START;

    if (inv_get_state() < INV_STATE_DMP_OPENED)
        return INV_ERROR_SM_IMPROPER_STATE;

    switch (controlSignal) {
    case INV_CONTROL_1:
        cntrl_params.gridThreshold[0] = threshold;
        break;
    case INV_CONTROL_2:
        cntrl_params.gridThreshold[1] = threshold;
        break;
    case INV_CONTROL_3:
        cntrl_params.gridThreshold[2] = threshold;
        break;
    case INV_CONTROL_4:
        cntrl_params.gridThreshold[3] = threshold;
        break;
    default:
        return INV_ERROR_INVALID_PARAMETER;
        //break;
    }

    return INV_SUCCESS;
}

/**
 *  @brief  inv_set_grid_max is used to set the maximum grid number for a control signal.
 *          inv_set_grid_max is used to adjust the maximum allowed grid number, above
 *          which the grid number will not be incremented.
 *          The minimum grid number is always zero.
 *
 *  @pre    inv_dmp_open() Must be called with MLDmpDefaultOpen() or 
 *          inv_open_low_power_pedometer().
 *
 *  @param controlSignal    Indicates which control signal is being modified.
 *                          Must be one of:
 *                          - INV_CONTROL_1,
 *                          - INV_CONTROL_2,
 *                          - INV_CONTROL_3 and
 *                          - INV_CONTROL_4.
 *
 *  @param  maximum         The maximum grid number for a control signal.
 *  @return Zero if the command is successful; an ML error code otherwise.
 */

inv_error_t inv_set_grid_max(unsigned short controlSignal, long maximum)
{
    INVENSENSE_FUNC_START;

    if (inv_get_state() != INV_STATE_DMP_OPENED)
        return INV_ERROR_SM_IMPROPER_STATE;

    switch (controlSignal) {
    case INV_CONTROL_1:
        cntrl_params.gridMaximum[0] = maximum;
        break;
    case INV_CONTROL_2:
        cntrl_params.gridMaximum[1] = maximum;
        break;
    case INV_CONTROL_3:
        cntrl_params.gridMaximum[2] = maximum;
        break;
    case INV_CONTROL_4:
        cntrl_params.gridMaximum[3] = maximum;
        break;
    default:
        return INV_ERROR_INVALID_PARAMETER;
        //break;
    }

    return INV_SUCCESS;
}

/**
 *  @brief  GridCallback function pointer type, to be passed as argument of 
 *          inv_set_grid_callback.
 *
 *  @param  controlSignal   Indicates which control signal crossed a grid threshold.
 *                          Must be one of:
 *                          - INV_CONTROL_1,
 *                          - INV_CONTROL_2,
 *                          - INV_CONTROL_3 and
 *                          - INV_CONTROL_4.
 *
 *  @param  gridNumber  An array of four numbers representing the grid number for each
 *                      control signal.
 *  @param  gridChange  An array of four numbers representing the change in grid number
 *                      for each control signal.
**/
typedef void (*fpGridCb) (unsigned short controlSignal, long *gridNum,
                          long *gridChange);

/**
 *  @brief  inv_set_grid_callback is used to register a callback function that
 *          will trigger when the grid location changes.
 *          inv_set_grid_callback allows a user to define a callback function that will
 *          run when a control signal crosses a grid threshold.

 *  @pre    inv_dmp_open() Must be called with MLDmpDefaultOpen() or 
 *          inv_open_low_power_pedometer().  inv_dmp_start must <b>NOT</b> have 
 *          been called.
 *
 *  @param  func    A user defined callback function
 *  @return Zero if the command is successful; an ML error code otherwise.
**/
inv_error_t inv_set_grid_callback(fpGridCb func)
{
    INVENSENSE_FUNC_START;

    if (inv_get_state() != INV_STATE_DMP_OPENED)
        return INV_ERROR_SM_IMPROPER_STATE;

    cntrl_params.gridCallback = func;
    return INV_SUCCESS;
}

/**
 *  @brief  inv_set_control_data is used to assign physical parameters to control signals.
 *          inv_set_control_data allows flexibility in assigning physical parameters to
 *          control signals. For example, the user is allowed to use raw gyroscope data
 *          as an input to the control algorithm.
 *          Alternatively, angular velocity can be used, which combines gyroscopes and
 *          accelerometers to provide a more robust physical parameter. Finally, angular
 *          velocity in world coordinates can be used, providing a control signal in
 *          which pitch and yaw are provided relative to gravity.
 *
 *  @pre    inv_dmp_open() Must be called with MLDmpDefaultOpen() or 
 *          inv_open_low_power_pedometer().
 *
 *  @param  controlSignal   Indicates which control signal is being modified.
 *                          Must be one of:
 *                          - INV_CONTROL_1,
 *                          - INV_CONTROL_2,
 *                          - INV_CONTROL_3 or
 *                          - INV_CONTROL_4.
 *
 *  @param  parameterArray   Indicates which parameter array is being assigned to a
 *                          control signal. Must be one of:
 *                          - INV_GYROS,
 *                          - INV_ANGULAR_VELOCITY, or
 *
 *  @param  parameterAxis   Indicates which axis of the parameter array will be used.
 *                          Must be:
 *                          - INV_ROLL,
 *                          - INV_PITCH, or
 *                          - INV_YAW.
 */

inv_error_t inv_set_control_data(unsigned short controlSignal,
                                 unsigned short parameterArray,
                                 unsigned short parameterAxis)
{
    INVENSENSE_FUNC_START;
    unsigned char regs[2] = { DINA80 + 10, DINA20 };
    inv_error_t result;

    if (inv_get_state() != INV_STATE_DMP_OPENED)
        return INV_ERROR_SM_IMPROPER_STATE;

    if (parameterArray == INV_ANGULAR_VELOCITY) {
        regs[0] = DINA80 + 5;
        regs[1] = DINA00;
    }
    switch (controlSignal) {
    case INV_CONTROL_1:
        cntrl_params.parameterArray[0] = parameterArray;
        switch (parameterAxis) {
        case INV_PITCH:
            regs[1] += 0x02;
            cntrl_params.parameterAxis[0] = 0;
            break;
        case INV_ROLL:
            regs[1] = DINA22;
            cntrl_params.parameterAxis[0] = 1;
            break;
        case INV_YAW:
            regs[1] = DINA42;
            cntrl_params.parameterAxis[0] = 2;
            break;
        default:
            return INV_ERROR_INVALID_PARAMETER;
        }
        result = inv_set_mpu_memory(KEY_CFG_3, 2, regs);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        break;
    case INV_CONTROL_2:
        cntrl_params.parameterArray[1] = parameterArray;
        switch (parameterAxis) {
        case INV_PITCH:
            regs[1] += DINA0E;
            cntrl_params.parameterAxis[1] = 0;
            break;
        case INV_ROLL:
            regs[1] += DINA2E;
            cntrl_params.parameterAxis[1] = 1;
            break;
        case INV_YAW:
            regs[1] += DINA4E;
            cntrl_params.parameterAxis[1] = 2;
            break;
        default:
            return INV_ERROR_INVALID_PARAMETER;
        }
        result = inv_set_mpu_memory(KEY_CFG_3B, 2, regs);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        break;
    case INV_CONTROL_3:
        cntrl_params.parameterArray[2] = parameterArray;
        switch (parameterAxis) {
        case INV_PITCH:
            regs[1] += DINA0E;
            cntrl_params.parameterAxis[2] = 0;
            break;
        case INV_ROLL:
            regs[1] += DINA2E;
            cntrl_params.parameterAxis[2] = 1;
            break;
        case INV_YAW:
            regs[1] += DINA4E;
            cntrl_params.parameterAxis[2] = 2;
            break;
        default:
            return INV_ERROR_INVALID_PARAMETER;
        }
        result = inv_set_mpu_memory(KEY_CFG_3C, 2, regs);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        break;
    case INV_CONTROL_4:
        cntrl_params.parameterArray[3] = parameterArray;
        switch (parameterAxis) {
        case INV_PITCH:
            regs[1] += DINA0E;
            cntrl_params.parameterAxis[3] = 0;
            break;
        case INV_ROLL:
            regs[1] += DINA2E;
            cntrl_params.parameterAxis[3] = 1;
            break;
        case INV_YAW:
            regs[1] += DINA4E;
            cntrl_params.parameterAxis[3] = 2;
            break;
        default:
            return INV_ERROR_INVALID_PARAMETER;
        }
        result = inv_set_mpu_memory(KEY_CFG_3D, 2, regs);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        break;
    default:
        result = INV_ERROR_INVALID_PARAMETER;
        break;
    }
    return result;
}

/**
 *  @brief  inv_get_control_data is used to get the current control data.
 *
 *  @pre    inv_dmp_open() Must be called with MLDmpDefaultOpen() or 
 *          inv_open_low_power_pedometer().
 *
 *  @param  controlSignal   Indicates which control signal is being queried.
 *                          Must be one of:
 *                          - INV_CONTROL_1,
 *                          - INV_CONTROL_2,
 *                          - INV_CONTROL_3 or
 *                          - INV_CONTROL_4.
 *
 *  @param  gridNum     A pointer to pass gridNum info back to the user.
 *  @param  gridChange  A pointer to pass gridChange info back to the user.
 *
 *  @return Zero if the command is successful; an ML error code otherwise.
 */

inv_error_t inv_get_control_data(long *controlSignal, long *gridNum,
                                 long *gridChange)
{
    INVENSENSE_FUNC_START;
    int_fast8_t i = 0;

    if (inv_get_state() != INV_STATE_DMP_STARTED)
        return INV_ERROR_SM_IMPROPER_STATE;

    for (i = 0; i < 4; i++) {
        controlSignal[i] = cntrl_obj.controlInt[i];
        gridNum[i] = cntrl_obj.gridNum[i];
        gridChange[i] = cntrl_obj.gridChange[i];
    }
    return INV_SUCCESS;
}

/** 
 * @internal
 * @brief   Update the ML Control engine.  This function should be called 
 *          every time new data from the MPU becomes available.
 *          Control engine outputs are written to the cntrl_obj data 
 *          structure.
 * @return  INV_SUCCESS or an error code.
**/
inv_error_t inv_update_control(struct inv_obj_t * inv_obj)
{
    INVENSENSE_FUNC_START;
    unsigned char i;
    long gridTmp;
    long tmp;

    inv_get_cntrl_data(cntrl_obj.mlGridNumDMP);

    for (i = 0; i < 4; i++) {
        if (cntrl_params.functions & INV_GRID) {
            if (cntrl_params.functions & INV_HYSTERESIS) {
                cntrl_obj.mlGridNumDMP[i] += cntrl_obj.gridNumOffset[i];
            }
            cntrl_obj.mlGridNumDMP[i] =
                cntrl_obj.mlGridNumDMP[i] / 2 + 1073741824L;
            cntrl_obj.controlInt[i] =
                (cntrl_obj.mlGridNumDMP[i] %
                 (128 * cntrl_params.gridThreshold[i])) / 128;
            gridTmp =
                cntrl_obj.mlGridNumDMP[i] / (128 *
                                             cntrl_params.gridThreshold[i]);
            tmp = 1 + 16777216L / cntrl_params.gridThreshold[i];
            cntrl_obj.gridChange[i] = gridTmp - cntrl_obj.lastGridNum[i];
            if (cntrl_obj.gridChange[i] > tmp / 2) {
                cntrl_obj.gridChange[i] =
                    gridTmp - tmp - cntrl_obj.lastGridNum[i];
            } else if (cntrl_obj.gridChange[i] < -tmp / 2) {
                cntrl_obj.gridChange[i] =
                    gridTmp + tmp - cntrl_obj.lastGridNum[i];
            }
            if ((cntrl_params.functions & INV_HYSTERESIS)
                && (cntrl_obj.gridChange[i] != 0)) {
                if (cntrl_obj.gridChange[i] > 0) {
                    cntrl_obj.gridNumOffset[i] +=
                        128 * cntrl_params.gridThreshold[i];
                    cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2;
                }
                if (cntrl_obj.gridChange[i] < 0) {
                    cntrl_obj.gridNumOffset[i] -=
                        128 * cntrl_params.gridThreshold[i];
                    cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2;
                }
            }
            cntrl_obj.gridNum[i] += cntrl_obj.gridChange[i];
            if (cntrl_obj.gridNum[i] >= cntrl_params.gridMaximum[i]) {
                cntrl_obj.gridNum[i] = cntrl_params.gridMaximum[i];
                if (cntrl_obj.controlInt[i] >=
                    cntrl_params.gridThreshold[i] / 2) {
                    cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2;
                }
            } else if (cntrl_obj.gridNum[i] <= 0) {
                cntrl_obj.gridNum[i] = 0;
                if (cntrl_obj.controlInt[i] < cntrl_params.gridThreshold[i] / 2) {
                    cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2;
                }
            }
            cntrl_obj.lastGridNum[i] = gridTmp;
            if ((cntrl_params.gridCallback) && (cntrl_obj.gridChange[i] != 0)) {
                cntrl_params.gridCallback((INV_CONTROL_1 << i),
                                          cntrl_obj.gridNum,
                                          cntrl_obj.gridChange);
            }

        } else {
            cntrl_obj.controlInt[i] = cntrl_obj.mlGridNumDMP[i];
        }

    }

    return INV_SUCCESS;
}

/**
 * @brief Enables the INV_CONTROL engine.
 *
 * @note  This function replaces MLEnable(INV_CONTROL)
 *
 * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must 
 *      have been called.
 *
 * @return INV_SUCCESS or non-zero error code
 */
inv_error_t inv_enable_control(void)
{
    INVENSENSE_FUNC_START;

    if (inv_get_state() != INV_STATE_DMP_OPENED)
        return INV_ERROR_SM_IMPROPER_STATE;

    memset(&cntrl_obj, 0, sizeof(cntrl_obj));

    inv_register_fifo_rate_process(inv_update_control, INV_PRIORITY_CONTROL);   // fixme, someone needs to send control data to the fifo
    return INV_SUCCESS;
}

/**
 * @brief Disables the INV_CONTROL engine.
 *
 * @note  This function replaces MLDisable(INV_CONTROL)
 *
 * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must 
 *      have been called.
 *
 * @return INV_SUCCESS or non-zero error code
 */
inv_error_t inv_disable_control(void)
{
    INVENSENSE_FUNC_START;

    if (inv_get_state() < INV_STATE_DMP_STARTED)
        return INV_ERROR_SM_IMPROPER_STATE;

    return INV_SUCCESS;
}

/**
 * @}
 */
